#ifndef SETTINGS_H
#define SETTINGS_H

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>

#include <sophus/se3.hpp>

#include <opencv2/core/persistence.hpp>

namespace msf
{

class Settings
{
public:
      /*
      * Enum for the different odom types implemented
      */
      enum OdomType {
            Two_Diff = 0

      };

      /*
      * Delete default constructor
      */
      Settings() = delete;

      /*
      * Constructor from file
      */
      Settings(const std::string &configFile, const int& sensor);

      /*
      * Ostream operator overloading to dump settings to the terminal
      */
      friend std::ostream &operator<<(std::ostream &output, const Settings &s);

      /*
      * Getter methods
      */
      float fps() {return fps_;}

      float noiseGyro() {return noiseGyro_;}
      float noiseAcc() {return noiseAcc_;}
      float gyroWalk() {return gyroWalk_;}
      float accWalk() {return accWalk_;}
      float imuFrequency() {return imuFrequency_;}
      Sophus::SE3f Tbo() {return Tbo_;}
      bool insertKPFsWhenLost() {return insertKPFsWhenLost_;}

      float keyPoseFrameSize() {return keyPoseFrameSize_;}
      float keyPoseFrameLineWidth() {return keyPoseFrameLineWidth_;}
      float graphLineWidth() {return graphLineWidth_;}
      float pointSize() {return pointSize_;}
      float odomSize() {return odomSize_;}
      float odomLineWidth() {return odomLineWidth_;}
      float viewPointX() {return viewPointX_;}
      float viewPointY() {return viewPointY_;}
      float viewPointZ() {return viewPointZ_;}
      float viewPointF() {return viewPointF_;}
      //float imageViewerScale() {return imageViewerScale_;}

private:
      template<typename T>
      T readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found,const bool required = true){
            cv::FileNode node = fSettings[name];
            if(node.empty()){
                  if(required){
                        std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
                        exit(-1);
                  }
                  else{
                        std::cerr << name << " optional parameter does not exist..." << std::endl;
                        found = false;
                        return T();
                  }

            }
            else{
                  found = true;
                  return (T) node;
            }
      }

      void readOdom(cv::FileStorage& fSettings);
      void readIMU(cv::FileStorage& fSettings);
      void readViewer(cv::FileStorage& fSettings);

      int sensor_;
      OdomType odomType_;     //Odom type

      /*
      * Odom stuff
      */
      float WheelRadius_;
      float ReductionRatio_;
      float BodyRadius_;
      float Frequency_;
      float fps_;

      /*
      * Inertial stuff
      */
      float noiseGyro_, noiseAcc_;
      float gyroWalk_, accWalk_;
      float imuFrequency_;
      Sophus::SE3f Tbo_;
      bool insertKPFsWhenLost_;

      /*
      * Viewer stuff
      */
      float keyPoseFrameSize_;
      float keyPoseFrameLineWidth_;
      float graphLineWidth_;
      float pointSize_;
      float odomSize_;
      float odomLineWidth_;
      float viewPointX_, viewPointY_, viewPointZ_, viewPointF_;
      //float imageViewerScale_;

};



}

#endif // SETTINGS_H